// swing the servos around to show them we're alive
// ------------------------------------------------
void demo_servos(byte i)
{
	while(i > 0){
		//gcs.send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
		rc.output_ch_pwm(1, 1400);
		delay(400);
		rc.output_ch_pwm(1, 1600);
		delay(200);
		rc.output_ch_pwm(1, 1500);
		delay(400);
		i--;
	}
}

void set_servo_mux(boolean mode)
{
	while(TCNT1 < 20000){};

	if (mode){
		//take over the MUX
		pinMode(4, OUTPUT);
		digitalWrite(4, HIGH);
	}else{
		//release the MUX to allow Manual Control
		digitalWrite(4, LOW);
		pinMode(4, INPUT);
	}
}

void output_manual()
{
	// do a direct pass through of radio values
	rc_1.radio_out 	= rc_1.radio_in;
	rc_2.radio_out 	= rc_2.radio_in;
	rc_3.radio_out 	= rc_3.radio_in;
	rc_4.radio_out 	= rc_4.radio_in;
	return;
}

// wants +- 45°
void set_servos_4()
{
	#if MIXING_MODE == 0
		rc_1.calc_pwm();
		rc_2.calc_pwm();
	#else
		/*Elevon mode*/
		float ch1;
		float ch2;
		ch1 = reverse_elevons * (rc_2.servo_out - rc_1.servo_out);
		ch2 = rc_2.servo_out + rc_1.servo_out;
		rc_1.radio_out =	elevon1_trim + (REVERSE_CH1_ELEVON * (ch1 * 500.0/ ROLL_SERVO_MAX));
		rc_2.radio_out =	elevon2_trim + (REVERSE_CH2_ELEVON * (ch2 * 500.0/ PITCH_SERVO_MAX));
	#endif


	#if THROTTLE_OUT == 0
		rc_3.servo_out = 0;
	#else
		rc_3.servo_out = constrain(rc_3.servo_out, THROTTLE_MIN, THROTTLE_MAX);
	#endif

	rc_3.calc_pwm();
	rc_4.calc_pwm();

	// send values to the PWM timers for output
	// ----------------------------------------
	rc.output_ch_pwm(CH_1, rc_1.radio_out); // send to Servos
	rc.output_ch_pwm(CH_2, rc_2.radio_out); // send to Servos
	rc.output_ch_pwm(CH_3, rc_3.radio_out); // send to Servos
	rc.output_ch_pwm(CH_4, rc_4.radio_out); // send to Servos

	//Serial.printf("roll: %ld\tci:%d\tso:%d\tro:%d\n", roll_sensor, rc_1.control_in, rc_1.servo_out, rc_1.radio_out);
	//print_control_in();
	//print_servo_out();
	//print_radio_out();
}
